#include"headfile.h"
#include"motor.h"
void motor_enable(){
    MOTOR_ENABLE=0;
}

void motor_disable(){
    MOTOR_ENABLE=1;
}

void motor_init(){
	pwm_init(PWM1P_P60, 16000, 0);
	pwm_init(PWM2P_P62, 16000, 0);
	pwm_init(PWM3P_P64, 16000, 0);
	pwm_init(PWM4P_P66, 16000, 0);
    pwm_out_left=0;
    pwm_out_right=0;
    motor_enable();
}

void motor_pwm_out(int16 speed_pwmL, int16 speed_pwmR){
	Limit(speed_pwmL,MOTOR_MAX);
    Limit(speed_pwmR,MOTOR_MAX);

    if(speed_pwmL > 0){
      pwm_duty(MOTOR_P2,(uint16)speed_pwmL);
      pwm_duty(MOTOR_P1,0);
    } 
    else{
       speed_pwmL = -speed_pwmL;
       pwm_duty(MOTOR_P1,(uint16)speed_pwmL);
       pwm_duty(MOTOR_P2,0);
    }
    
    if(speed_pwmR > 0){ 
       pwm_duty(MOTOR_P3,(uint16)speed_pwmR);
       pwm_duty(MOTOR_P4,0);
    } 
    else{
       speed_pwmR = -speed_pwmR;
       pwm_duty(MOTOR_P4,(uint16)speed_pwmR);
       pwm_duty(MOTOR_P3,0);
    }
}
//电机速度环
// PID_Struct PID_Left ={20,0,0,0,0,1000,0,0.005};
// PID_Struct PID_Right={20,0,0,0,0,1000,0,0.005};
// float speed_out_left, speed_out_right;
// void motor_set_pid(float p,float i,float d){
//     set_PID(&PID_Left,p,i,d);
//     set_PID(&PID_Right,p,i,d);
// }
// void motor_speed_out(float tagert_l,float tagert_r){
//     pwm_out_left = -(int16)PID_Release(&PID_Left, tagert_l, speed_left); //占空比方向与速度方向相反
//     pwm_out_right = -(int16)PID_Release(&PID_Right, tagert_r, speed_right);
//     motor_pwm_out(pwm_out_left,pwm_out_right); 
// }
// void motor_set_speed(float l,float r){
//     speed_out_left=l;
//     speed_out_right=r;
// }

PID_Struct PD_Angle = {400,0,90,0,0,0,0,0.005};
PID_Struct PI_Speed = {0,0,0,0,0,0,0,0.005};
PID_Struct PD_Direct = {3000,0,50,0,0,0,0,0.005};
int16 pwm_out_left, pwm_out_right;
// float pwm_out_speed,pwm_out_direct = 0;

float target_angle=9;

void motor_angle_pd(float p ,float d){
    PD_Angle.KP=p;
    PD_Angle.KD=d;
}
void motor_speed_pi(float p,float i){
    PI_Speed.KP=p;
    PI_Speed.KI=i;
}

void motor_angle_ctrl(){
    float pwm_float = PID_Release(&PD_Angle, target_angle, icm_out_angle);
    Limit(pwm_float,10000); //float 需限制一下大小 在强制转换较大的负数 会转为正数
    pwm_out_left =(int16) pwm_float;
    pwm_out_right=pwm_out_left;        
    // motor_pwm_out(-5000,-5000);
}
int16 speed_avg=0;
int16 speed_avg_last = 0;
void motor_speed_ctrl(){
    speed_avg = speed_left + speed_right;
	speed_avg *= 0.8;
	speed_avg += speed_avg_last * 0.2;
	speed_avg_last = speed_avg;
}

void motor_exe(){
    motor_angle_ctrl();
    motor_pwm_out(pwm_out_left,pwm_out_right);
    // motor_pwm_out(5000,5000);

}





// float err_last=0,err=0,err_int=0;
// float kp,ki;
// motor_speed_out(float targetspeed){
    
// }

// int velocity(int encoder_left,int encoder_right)
// {
// 	static float Velocity,Encoder_Least,Encoder;
// 	static float Encoder_Integral;
	
// 	float kp=80,ki=0.4;
// 	Encoder_Least =(Encoder_Left+Encoder_Right)-0;
// 	Encoder *= 0.7;
// 	Encoder += Encoder_Least*0.3;
// 	Encoder_Integral +=Encoder;
// 	Velocity=Encoder*kp+Encoder_Integral*ki;
// 	return Velocity;
// }



